#pragma once
#include <Eigen/Dense>

#define DATA_COUNT 1
#define STATE_COUNT 3

#define OBSERVATION_NOISE_COVARIANCE 1e-3
#define PROCESS_NOISE_COVARIANCE 1e-2


class kalman_filter_hugo{
public:
    kalman_filter_hugo(/*
        const Eigen::Matrix<double, STATE_COUNT, 1> &init_priori_state_estimate,
        const Eigen::Matrix<double, STATE_COUNT, STATE_COUNT> &init_priori_estimate_covariance,
        const Eigen::Matrix<double, STATE_COUNT, STATE_COUNT> &init_state_transition_model,
        const Eigen::Matrix<double, STATE_COUNT, 1> &init_process_noise_apply_model,
        const Eigen::Matrix<double, STATE_COUNT, STATE_COUNT> &init_process_noise_covariance,
        const Eigen::Matrix<double, 1, STATE_COUNT> &init_observation_model,
        const Eigen::Matrix<double, 1, 1> &init_observation_noise_covariance*/
    );
    void update_kalman_filter(double update_time, double measure);
    double get_estimate_state();
    double get_predict_state(double future_time);

private:

    void predict_state_estimate();
    void predicted_estimate_covariance();
    void update_measurement_residual();
    void update_measurement_residual_covariance();
    void update_optimal_Kalman_gain();
    void update_posteriori_state_estimate(); // state update equation
    void update_posteriori_estimate_covariance();
    void predict_future_state(double future_time);
    
    void refresh_step_time(double __step_time__);
    void refresh_state_transition_model(double __step_time__); // State Extrapolation Equation
    void refresh_measurement(double measure);
    void refresh_process_noise_apply_model(double __step_time__);
    void refresh_observation_model(double __step_time__);

    void init_filter_state(double init_measure);
    void init_posteriori_state_estimate(double init_measure);
    void init_posteriori_estimate_covariance(double scaler);
    void init_observation_noise_covariance(double scaler);
    void init_process_noise_covariance(double scaler);

    Eigen::Matrix<double, STATE_COUNT, 1> priori_state_estimate;
    Eigen::Matrix<double, STATE_COUNT, 1> posteriori_state_estimate;
    Eigen::Matrix<double, STATE_COUNT, STATE_COUNT> priori_estimate_covariance;
    Eigen::Matrix<double, STATE_COUNT, STATE_COUNT> posteriori_estimate_covariance;
    Eigen::Matrix<double, STATE_COUNT, STATE_COUNT> state_transition_model;
    Eigen::Matrix<double, STATE_COUNT, 1> process_noise_apply_model;
    Eigen::Matrix<double, STATE_COUNT, STATE_COUNT> process_noise_covariance;
    Eigen::Matrix<double, DATA_COUNT, 1> measurement;
    Eigen::Matrix<double, 1, STATE_COUNT> observation_model;
    Eigen::Matrix<double, 1, 1> measurement_residual;
    Eigen::Matrix<double, 1, 1> observation_noise_covariance;
    Eigen::Matrix<double, 1, 1> measurement_residual_covariance;
    Eigen::Matrix<double, STATE_COUNT, 1> optimal_Kalman_gain;
    double last_update_time;
    double step_time;
    Eigen::Matrix<double, STATE_COUNT, 1> predict_state;

    uint8_t kalman_filter_init_state;
};